# 取色器节点
# 用于测量目标颜色HSV数值
# 2024/03/18

import cv2
from cv_bridge import CvBridge, CvBridgeError
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image


# 定义取色器节点
class ColorPickerNode(Node):

    def __init__(self):
        super().__init__('color_picker_node')
        self.bridge = CvBridge()
        # 订阅/camera/image_color话题
        self.subscription = self.create_subscription(
            Image,
            '/camera/image_color',
            self.image_callback,
            10)
        self.subscription  # prevent unused variable warning

    # 定义回调函数
    def image_callback(self,msg):
        # 使用cv_bridge将ROS的图像数据转换成OpenCV的图像格式
        try:
            cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
            cv_image = cv2.resize(cv_image, (1280, 720))
            HSV=cv2.cvtColor(cv_image,cv2.COLOR_BGR2HSV)
        except CvBridgeError as e:
            print (e)
        cv2.rectangle(cv_image,(640-10,360-10),(640+10,360+10),(0,255,0),2) # 在画面中央绘制矩形
        cv2.imshow("cv_image",cv_image)
        cv2.waitKey(1)
        print(HSV[360,640]) # 打印中心点HSV数值


def main(args=None):
    try:
        rclpy.init(args=args)
        color_picker_node = ColorPickerNode()
        rclpy.spin(color_picker_node)
    except KeyboardInterrupt: # 关闭窗口
        print("Shutting down color picker node.")
        cv2.destroyAllWindows()


if __name__ == '__main__':
    main()
